#include "udf_global.h"

#if ( UDF_EN_01 > 0 && !defined(UDP_SUB_PROJECT) && !defined(UDT_SUB_PROJECT) && (defined(UDF01_SUB_PROJECT) || defined(UDF_PRJ)) )

#include "udsf.h"

#include "../udsf/udsf_uart.c"
#include "../udsf/udsf_spi.c"
#include "../udsf/udsf_udf_switch.c"
#include "../udsf/udsf_lpw.c"
//#include "../udsf/udsf_protocol.c"
#include "../udsf/udsf_dbscan.c"
#include "../udsf/udsf_delay.c"

#if (POINTRST_USER_PROCESS == 1)
#include "../udsf/udsf_pointrst.c"
#include "udf_global.h"
#endif

	
// 基于UDF03/UDF13点云结果继续处理
void UDF01_udf_handle(void){
			MaxPowPoint maxpower={0};
			MaxPowPoint pre_maxpower={0};
			TrackInfo_XY Track_xy={0};
			TrackInfo_XY Pre_Track_xy={0};
			uint8_t trigerAngle[6] = {29, 51, 72, 88, 97,101};
			uint16_t trigerDistance[5] = {300, 280, 250, 220, 200};
			uint8_t stilmaxpowersign = 0;			
			uint8_t slowQuitSum = 0;
			uint8_t correction_flag = 1;
					
			STRUCT_POINTRST *frameRst = (STRUCT_POINTRST *)(UDFDEF_ADDR_POINT_RST);
			STRUCT_POINTRST *frameRst_presence = (STRUCT_POINTRST *)(UDFDEF_ADDR_POINT_RST_PRESENCE);

			struct CtrackList_tmp *ctrackOut = (struct CtrackList_tmp *)(UDFDEF_ADDR_POINT_RST + 84);
			struct CtrackList_tmp *ctrackOut_presence = (struct CtrackList_tmp *)(UDFDEF_ADDR_POINT_RST + 144 );
//			struct Presence_Flag_Info_tmp *judgeBuff = (struct Presence_Flag_Info_tmp *)(UDFDEF_ADDR_POINT_RST+100);
#if 0	
//	    if(frameRst->targetNum)
//				printf("\r\nDel: R1 = %d, V1 = %d, A1= %d, P1 = %d",frameRst->target[0].d1Idx,frameRst->target[0].d2Idx,frameRst->target[0].d3Idx, frameRst->target[0].powABS);				
			if(frameRst_presence->targetNum)
				printf("\r\nDel: R2 = %d, V2 = %d, A2= %d, P2 = %d",frameRst_presence->target[0].d1Idx,frameRst_presence->target[0].d2Idx,frameRst_presence->target[0].d3Idx, frameRst_presence->target[0].powABS);	
#endif
				if(frameRst->targetNum){   //动点
				maxpower.maxPowR = (int32_t)frameRst->target[0].d1Idx* RANGETHRES;//
				maxpower.maxPowV = (int32_t)frameRst->target[0].d2Idx* VELTHRES;
				uint8_t temp = udsf_abs(frameRst->target[0].d3Idx);
				if(temp> 20 && temp<26)
					frameRst->target[0].d3Idx = frameRst->target[0].d3Idx+3;
				maxpower.maxPowA = (int32_t)frameRst->target[0].d3Idx* 200/NUM_ANGLE;
				maxpower.maxPow  = (int32_t)frameRst->target[0].powABS;
//				printf("\r\nr=%d, v=%d, a=%d, p=%d,th=%d", maxpower.maxPowR, maxpower.maxPowV, maxpower.maxPowA, maxpower.maxPow,trigerAngle[paramRSV->triAngle-1]);
				
				if(maxpower.maxPowA > trigerAngle[paramRSV->triAngle-1] || maxpower.maxPowA < -trigerAngle[paramRSV->triAngle-1]){
					maxpower.maxPowR = 0;
					maxpower.maxPowV = 0;
					maxpower.maxPowA = 0;
					maxpower.maxPow  = 0;
				}
			}
				if(frameRst_presence->targetNum){  //静点
				pre_maxpower.maxPowR = (int32_t)frameRst_presence->target[0].d1Idx * RANGETHRES;
				pre_maxpower.maxPowV = (int32_t)frameRst_presence->target[0].d2Idx * VELTHRES;
				pre_maxpower.maxPowA = (int32_t)frameRst_presence->target[0].d3Idx * 200/NUM_ANGLE;
				pre_maxpower.maxPow  = (int32_t)frameRst_presence->target[0].powABS;
//				printf("\r\nr1=%d, v1=%d, a1=%d, p1=%d", pre_maxpower.maxPowR, pre_maxpower.maxPowV, pre_maxpower.maxPowA, pre_maxpower.maxPow);
				
				if(pre_maxpower.maxPowA > trigerAngle[paramRSV->triAngle-1] || pre_maxpower.maxPowA < -trigerAngle[paramRSV->triAngle-1]){
					pre_maxpower.maxPowR = 0;
					pre_maxpower.maxPowV = 0;
					pre_maxpower.maxPowA = 0;
					}
				}
//				if(pre_maxpower.maxPowR>130)
//					pre_maxpower.maxPowR=pre_maxpower.maxPowR-30;
				udsf_calculatePoint_RXY(pre_maxpower.maxPowR,pre_maxpower.maxPowA* NUM_ANGLE/200,&Pre_Track_xy.r,&Pre_Track_xy.x,&Pre_Track_xy.y);

				if(pre_maxpower.maxPow > 5000 && pre_maxpower.maxPowR < trigerDistance[paramRSV->triDisLevel-1] + 30  && paramRSV->bodyInFlag == 1) //若静帧大于多少，是有人										
					stilmaxpowersign = 1;				
				else
					stilmaxpowersign = 0;
				
				for(uint8_t i=2;i<5;i++){
					ctrackOut_presence->info[i] = ctrackOut_presence->info[i+1];
				}
				ctrackOut_presence->info[5].r = Pre_Track_xy.r;
				ctrackOut_presence->info[5].x = Pre_Track_xy.x;
				ctrackOut_presence->info[5].y = Pre_Track_xy.y;
//				printf("\r\nr2=%d,x2=%d,y2=%d",Pre_Track_xy.r,Pre_Track_xy.x,Pre_Track_xy.y);					
				uint8_t ValidCnt = 0;
				memset((uint8_t *)(UDFDEF_ADDR_POINT_RST + 148), 0, 8); //必须清
				for(uint8_t i=2;i<6;i++){
					if(ctrackOut_presence->info[i].x||ctrackOut_presence->info[i].y){
						ValidCnt++;		
						ctrackOut_presence->info[0].r = ctrackOut_presence->info[0].r + ctrackOut_presence->info[i].r;
						ctrackOut_presence->info[0].x = ctrackOut_presence->info[0].x + ctrackOut_presence->info[i].x;
						ctrackOut_presence->info[0].y = ctrackOut_presence->info[0].y + ctrackOut_presence->info[i].y;
					}					
				}
				if(ValidCnt){
				ctrackOut_presence->info[0].r  = ctrackOut_presence->info[0].r / ValidCnt;
				ctrackOut_presence->info[0].x  = ctrackOut_presence->info[0].x / ValidCnt;
				ctrackOut_presence->info[0].y  = ctrackOut_presence->info[0].y / ValidCnt;
				}
//				for(uint8_t i = 0;i<7;i++)
//					printf("\r\n[%d]r1=%d, x1=%d, y1=%d",i, ctrackOut_presence->info[i].r , ctrackOut_presence->info[i].x ,ctrackOut_presence->info[i].y );
				
				if(paramRSV->bodyInFlag == 1 && (sUDF_global.track.track_sign == 0 || sUDF_global.track.track_r > trigerDistance[paramRSV->triDisLevel-1] + 30)) //追踪不到人或人在触发距离外，frameDiff进行自加
				{
					paramRSV->frameDiff ++;
				}
				else
				{
					paramRSV->frameDiff = 0;
					udsf_memset(paramRSV->judgeBuff,0,ABNORMAL_OUT_THRES);
				}
				
				if(paramRSV->bodyInFlag == 1 && paramRSV->frameDiff > 32)    //将judgeBuff数据轮动往前移
				{
					for(uint8_t i=0;i < ABNORMAL_OUT_THRES-1;i++)
					{								
						paramRSV->judgeBuff[i] = paramRSV->judgeBuff[i+1];
						slowQuitSum = slowQuitSum + paramRSV->judgeBuff[i]; //这里计算judgeBuff数组和
					}
				}
				if(paramRSV->bodyInFlag == 1 && (sUDF_global.track.track_sign == 0 || sUDF_global.track.track_r > trigerDistance[paramRSV->triDisLevel-1] + 30)&& maxpower.maxPowSign == 0 
					&& stilmaxpowersign == 0 && paramRSV->frameDiff > 32 )
				{
					paramRSV->judgeBuff[ABNORMAL_OUT_THRES-1] = 1;
					slowQuitSum = slowQuitSum + paramRSV->judgeBuff[ABNORMAL_OUT_THRES-1];
				}
				else
				{							
					paramRSV->judgeBuff[ABNORMAL_OUT_THRES-1] = 0;
					slowQuitSum = slowQuitSum + paramRSV->judgeBuff[ABNORMAL_OUT_THRES-1];
				}
					
#if 0	
	    if(frameRst->targetNum)
				printf("\r\nDel: R1 = %d, V1 = %d, A1= %d, P1 = %d",maxpower.maxPowR,maxpower.maxPowV,maxpower.maxPowA, maxpower.maxPow);				
			if(frameRst_presence->targetNum)
				printf("\r\nDel: R2 = %d, V2 = %d, A2= %d, P2 = %d",pre_maxpower.maxPowR,pre_maxpower.maxPowV,pre_maxpower.maxPowA, pre_maxpower.maxPow)
#endif
			
				if(maxpower.maxPow > paramRSV->powThres && sUDF_global.track.track_sign == 0 && maxpower.maxPowR > MIN_RANGE && maxpower.maxPowR < MAX_RANGE)
					{
						maxpower.maxPowSign = 1;
					}
				else if (maxpower.maxPow > paramRSV->powThres && sUDF_global.track.track_sign == 1&& (sUDF_global.track.track_r - maxpower.maxPowR) > -MAX_RANGE_DEV
						&& (sUDF_global.track.track_r - maxpower.maxPowR) < MAX_RANGE_DEV
						&& maxpower.maxPowR > MIN_RANGE && maxpower.maxPowR < MAX_RANGE)
					{
						maxpower.maxPowSign = 1;
					}
					else
					{
						maxpower.maxPowSign = 0;
					}
				udsf_track_FastIn(&maxpower, &sUDF_global.track, paramRSV, paramRSV->bodyInFlag);
	
				udsf_calculatePoint_RXY(sUDF_global.track.track_r,sUDF_global.track.track_a* NUM_ANGLE/200,&Track_xy.r,&Track_xy.x,&Track_xy.y);
			
				for(uint8_t i=2;i<5;i++){
					ctrackOut->info[i] = ctrackOut->info[i+1];
				}
					
				ctrackOut->info[5].r = sUDF_global.track.track_r;
				ctrackOut->info[5].x = Track_xy.x;
				ctrackOut->info[5].y = Track_xy.y;
				
				ValidCnt = 0;
				memset((uint8_t *)(UDFDEF_ADDR_POINT_RST + 88), 0, 8); //必须清
				for(uint8_t i=2;i<6;i++){
					if(ctrackOut->info[i].x||ctrackOut->info[i].y){
						ValidCnt++;
						ctrackOut->info[0].r = ctrackOut->info[0].r + ctrackOut->info[i].r;
						ctrackOut->info[0].x = ctrackOut->info[0].x + ctrackOut->info[i].x;
						ctrackOut->info[0].y = ctrackOut->info[0].y + ctrackOut->info[i].y;
					}					
				}
				if(ValidCnt>1){
				ctrackOut->info[0].r  = ctrackOut->info[0].r / ValidCnt;
				ctrackOut->info[0].x  = ctrackOut->info[0].x / ValidCnt;
				ctrackOut->info[0].y  = ctrackOut->info[0].y / ValidCnt;
				}
				ctrackOut->info[0].r=(ctrackOut->info[0].r*7+ ctrackOut->info[6].r*3)/10;
			  ctrackOut->info[0].x=(ctrackOut->info[0].x*7+ ctrackOut->info[6].x*3)/10;
				ctrackOut->info[0].y=(ctrackOut->info[0].y*7+ ctrackOut->info[6].y*3)/10;
				
				ctrackOut->info[1]  = ctrackOut_presence->info[0];//存静点
				
				uint8_t flag = !(ctrackOut->info[3].r && ctrackOut->info[4].r && ctrackOut->info[5].r);
				if( flag && paramRSV->frameDiff > 0 && paramRSV->frameDiff < 32 ) //动帧消失32帧内定死
				{
					ctrackOut->info[0].r=(ctrackOut->info[1].r+ ctrackOut->info[6].r*39)/40;
					ctrackOut->info[0].x=(ctrackOut->info[1].x+ ctrackOut->info[6].x*39)/40;
					ctrackOut->info[0].y=(ctrackOut->info[1].y+ ctrackOut->info[6].y*39)/40;
					correction_flag = 0;
					
				}
				else if( flag && paramRSV->frameDiff >= 32 ){  //若32帧后，还有静帧，动帧继续定死
					ctrackOut->info[0].r=(ctrackOut->info[1].r+ ctrackOut->info[6].r*19)/20;
					ctrackOut->info[0].x=(ctrackOut->info[1].x+ ctrackOut->info[6].x*19)/20;
					ctrackOut->info[0].y=(ctrackOut->info[1].y+ ctrackOut->info[6].y*19)/20;
					correction_flag = 0;
				}
//				for(uint8_t i = 0;i<7;i++)
//					printf("\r\n[%d]r2=%d, x2=%d, y2=%d",i, ctrackOut->info[i].r , ctrackOut->info[i].x ,ctrackOut->info[i].y );				
				if(paramRSV->bodyInFlag == 1 && ctrackOut->info[0].r >  trigerDistance[paramRSV->triDisLevel-1]  + 40 && stilmaxpowersign == 0)//快速退出范围，触发距离外60cm
						paramRSV->Outside_trigger_range ++;
					else
						paramRSV->Outside_trigger_range = 0;
//				printf("\noutrange=%d",paramRSV->Outside_trigger_range);
												
				if(maxpower.maxPowSign == 1 && ctrackOut->info[0].r > 0 
					&& ctrackOut->info[0].r < trigerDistance[paramRSV->triDisLevel-1] + 30 
				  && paramRSV->bodyInFlag ==0) //// 连续2帧进入
				{
					paramRSV->bodyInCnt ++;							
				}
				else
				{
					paramRSV->bodyInCnt = 0;
				}
					
				if(paramRSV->bodyInFlag ==0 && paramRSV->bodyInCnt>=2){
						paramRSV->bodyInFlag =1 ;
						ctrackOut->status = 1;
						paramRSV->bodyInCnt = 0;
				}	
				else if(paramRSV->bodyInFlag == 1 && paramRSV->Outside_trigger_range > 10 ) { //单人快速退出
						paramRSV->bodyInFlag =0 ;
						ctrackOut->status = 3;
				}				
				else if(paramRSV->bodyInFlag == 1 && slowQuitSum > ABNORMAL_OUT_THRES - 5){//慢退出
					paramRSV->bodyInFlag =0 ;
					ctrackOut->status = 3;
				}
				else if(paramRSV->bodyInFlag ==1 ){
					ctrackOut->status = 2;
				}
#if 1		
				{
//					if(ctrackOut->info[0].r>trigerDistance[paramRSV->triDisLevel-1]+30)
//					{
//					ctrackOut->info[0].r=0;
//					ctrackOut->info[0].x=0;
//					ctrackOut->info[0].y=0;
//					}
					udsf_uart_send_byte(0x55);
					udsf_uart_send_byte(0xAA);
					ctrackOut->nTrack = 1;					
					uint32_t sendLen = ctrackOut->nTrack * 8 + 4;
					udsf_uart_send_data((uint8_t *)&sendLen, 4);

					udsf_uart_send_data((uint8_t *)&ctrackOut->status, 2);
					udsf_uart_send_data((uint8_t *)&ctrackOut->nTrack, 2);
					udsf_uart_send_data((uint8_t *)ctrackOut->info, ctrackOut->nTrack * 8);
				}
#endif
//		printf ("it is ok\n");
	if(ctrackOut->info[0].r && correction_flag)			
		ctrackOut->info[6] = ctrackOut->info[0]; //存最后一帧有数据的动态点

				
	udsf_udf_switch_without_crc(3,1); // udf01->udf03 
}
#endif
